#pragma once

#define I_DEFAULT_WARP_BLANK_VALUE_UCHAR  0
#define I_WARP_COORDINATE_SHIFT_STEP      15 /*k*/
#define I_WARP_COORDINATE_POWER_TWO       32768 /*2^k*/
#define I_WARP_COORDINATE_FRACTIONAL_MASK 32767 /*2^k-1*/
#define I_WARP_COORDINATE_ROUNDADD        16384 /*2^(k-1)*/

namespace idl
{
  template<typename T>
  inline void i_warp_affine_convert_to_int(const T A[6], int Aint[6])
  {
    Aint[0] = (int)(A[0] * I_WARP_COORDINATE_POWER_TWO);
    Aint[1] = (int)(A[1] * I_WARP_COORDINATE_POWER_TWO);
    Aint[2] = (int)(A[2] * I_WARP_COORDINATE_POWER_TWO);
    Aint[3] = (int)(A[3] * I_WARP_COORDINATE_POWER_TWO);
    Aint[4] = (int)(A[4] * I_WARP_COORDINATE_POWER_TWO);
    Aint[5] = (int)(A[5] * I_WARP_COORDINATE_POWER_TWO);
  }

  /*test four corner points to check if the warpped image is completely inside the image canvas*/
  inline int i_warp_affine_is_inside(const int Aint[6], int width, int height)
  {
    int xc, yc, a0wm1, a3wm1;
    int wm1 = width - 1;
    int hm1 = height - 1;
    
    /*check (0,0)*/
    xc = Aint[2]; 
    yc = Aint[5]; 
    if (xc<0) return(0);
    if ((xc >> I_WARP_COORDINATE_SHIFT_STEP) >= wm1) return(0);
    if (yc<0) return(0);
    if ((yc >> I_WARP_COORDINATE_SHIFT_STEP) >= hm1) return(0);
    
    /*check (w-1, 0)*/
    a0wm1 = wm1*Aint[0];
    a3wm1 = wm1*Aint[3];
    xc += a0wm1;
    yc += a3wm1;
    if (xc<0) return(0);
    if ((xc >> I_WARP_COORDINATE_SHIFT_STEP) >= wm1) return(0);
    if (yc<0) return(0);
    if ((yc >> I_WARP_COORDINATE_SHIFT_STEP) >= hm1) return(0);
    
    /*check (w-1, h-1)*/
    xc += hm1*Aint[1]; 
    yc += hm1*Aint[4];
    if (xc<0) return(0);
    if ((xc >> I_WARP_COORDINATE_SHIFT_STEP) >= wm1) return(0);
    if (yc<0) return(0);
    if ((yc >> I_WARP_COORDINATE_SHIFT_STEP) >= hm1) return(0);
    
    /*check (0, h-1)*/
    xc -= a0wm1;
    yc -= a3wm1;
    if (xc<0) return(0);
    if ((xc >> I_WARP_COORDINATE_SHIFT_STEP) >= wm1) return(0);
    if (yc<0) return(0);
    if ((yc >> I_WARP_COORDINATE_SHIFT_STEP) >= hm1) return(0);
    
    return(1);
  }

  /*Warp into the row d with w entries, from the source image src.
  The pixel mapping is dst[i]=s[yinc*i+yc][xinc*i+xc])
  i.e.,
  A=
  [xinc xc]
  [yinc yc]
  is an affine mapping from d to src.*/
  inline void i_warpline_lum_affine_bilinear_unclipped(const unsigned char *const*src, 
                                                       unsigned char *d, 
                                                       int xc, int yc, int xinc, int yinc, int w)
  {
    int i, xi, yi, xip, xfrac, yfrac, omxfrac, omyfrac;
    const unsigned char *syi, *syip;
    /*Start at (xc,yc)*/
    for (i = 0; i<w; i++)
    {
      xi = (xc >> I_WARP_COORDINATE_SHIFT_STEP);
      yi = (yc >> I_WARP_COORDINATE_SHIFT_STEP);
      xfrac = xc & I_WARP_COORDINATE_FRACTIONAL_MASK;
      yfrac = yc & I_WARP_COORDINATE_FRACTIONAL_MASK;
      xip = (xi + 1);

      syi  = src[yi];
      syip = src[yi + 1];

      omxfrac = I_WARP_COORDINATE_FRACTIONAL_MASK - xfrac;
      omyfrac = I_WARP_COORDINATE_FRACTIONAL_MASK - yfrac;
      
      d[i] = (unsigned char)
        (
        (omyfrac*((omxfrac* syi[xi] + xfrac* syi[xip] + I_WARP_COORDINATE_ROUNDADD) >> I_WARP_COORDINATE_SHIFT_STEP) +
           yfrac*((omxfrac*syip[xi] + xfrac*syip[xip] + I_WARP_COORDINATE_ROUNDADD) >> I_WARP_COORDINATE_SHIFT_STEP)
         + I_WARP_COORDINATE_ROUNDADD) >> I_WARP_COORDINATE_SHIFT_STEP);

      /*Move coordinates one step further along*/
      xc += xinc; 
      yc += yinc;
    }
  }

  /*Warp, filling at the beginning and end of line with blanks*/
  inline void i_warpline_lum_affine_bilinear_clipped(const unsigned char *const*src,
                                                     unsigned char *d, 
                                                     int xc, int yc, int xinc, int yinc, int w, int h, unsigned char blank)
  {
    int i, xi, yi, xip, xfrac, yfrac, omxfrac, omyfrac, xct, yct, wm1, hm1, t;
    const unsigned char *syi, *syip;

    wm1 = w - 1;
    hm1 = h - 1;
    /*Start from end of line*/
    t = wm1;
    xct = xc + wm1*xinc;
    yct = yc + wm1*yinc;

    /*Run backwards and fill with blanks until all coordinates have been ok at least once from the
    end of line*/
    for (; (t >= 0) && (xct<0); t--) { d[t] = blank; xct -= xinc; yct -= yinc; }
    for (; (t >= 0) && (yct<0); t--) { d[t] = blank; xct -= xinc; yct -= yinc; }
    for (; (t >= 0) && ((xct >> I_WARP_COORDINATE_SHIFT_STEP) >= wm1); t--) { d[t] = blank; xct -= xinc; yct -= yinc; }
    for (; (t >= 0) && ((yct >> I_WARP_COORDINATE_SHIFT_STEP) >= hm1); t--) { d[t] = blank; xct -= xinc; yct -= yinc; }
    /*We must now in t have a point that corresponds to a valid pixel in the source, or the whole
    line is invalid due to a coordinate that used to be ok, but now is not. In the latter case,
    if we run from the beginning of the line, that coordinate will make sure that we dont do any warping.
    In the former case, there is symmetry, so there will either be no valid pixel, or we will also from
    the beginning find an actually valid pixel. In that case, all the pixels in between must be valid,
    so we are ok.
    /*Start from beginning of line and fill with blanks until all coordinates have been ok at least once
    from beginning of line*/
    for (i = 0; (i<w) && (xc<0); i++)     { d[i] = blank; xc += xinc; yc += yinc; }
    for (; (i<w) && (yc<0); i++)     { d[i] = blank; xc += xinc; yc += yinc; }
    for (; (i<w) && ((xc >> I_WARP_COORDINATE_SHIFT_STEP) >= wm1); i++) { d[i] = blank; xc += xinc; yc += yinc; }
    for (; (i<w) && ((yc >> I_WARP_COORDINATE_SHIFT_STEP) >= hm1); i++) { d[i] = blank; xc += xinc; yc += yinc; }
    
    /*Run from i up to and including t doing actual warping*/
    for (; i <= t; i++)
    {
      xi = (xc >> I_WARP_COORDINATE_SHIFT_STEP);
      yi = (yc >> I_WARP_COORDINATE_SHIFT_STEP);
      xfrac = xc & I_WARP_COORDINATE_FRACTIONAL_MASK;
      yfrac = yc & I_WARP_COORDINATE_FRACTIONAL_MASK;
      xip = (xi + 1);

      syi = src[yi];
      syip = src[(yi + 1)];

      omxfrac = I_WARP_COORDINATE_FRACTIONAL_MASK - xfrac;
      omyfrac = I_WARP_COORDINATE_FRACTIONAL_MASK - yfrac;
      d[i] = (unsigned char)
        (
        (omyfrac * ((omxfrac*syi[xi ] + xfrac*syi[xip ] + I_WARP_COORDINATE_ROUNDADD) >> I_WARP_COORDINATE_SHIFT_STEP) +
           yfrac * ((omxfrac*syip[xi] + xfrac*syip[xip] + I_WARP_COORDINATE_ROUNDADD) >> I_WARP_COORDINATE_SHIFT_STEP)
        + I_WARP_COORDINATE_ROUNDADD) >> I_WARP_COORDINATE_SHIFT_STEP);

      /*Move coordinates one step further along*/
      xc += xinc; yc += yinc;
    }
  }


  inline void i_warp_lum_affine_bilinear_unclipped(const unsigned char *const*src, const int Aint[6], unsigned char **dst, int width, int height)
  {
    int row, xc, yc, ai0, ai1, ai3, ai4;
    
    ai0 = Aint[0]; ai1 = Aint[1];
    xc  = Aint[2]; ai3 = Aint[3];
    ai4 = Aint[4]; yc  = Aint[5];
    
    /*Start coordinate (0,0)*/
    for (row = 0; row < height; row++)
    {
      /*Warp out one row*/
      i_warpline_lum_affine_bilinear_unclipped(src, dst[row], xc, yc, ai0, ai3, width);
      /*Move coordinates one step further along*/
      xc += ai1; yc += ai4;
    }
  }

  inline void i_warp_lum_affine_bilinear_clipped(const unsigned char *const*src, const int Aint[6], unsigned char **dst, int width, int height, unsigned char blank)
  {
    int row, xc, yc, ai0, ai1, ai3, ai4;

    ai0 = Aint[0]; ai1 = Aint[1];
    xc  = Aint[2]; ai3 = Aint[3];
    ai4 = Aint[4];  yc = Aint[5];

    /*Start coordinate (0,0)*/
    for (row = 0; row<height; row++)
    {
      /*Warp out one row*/
      i_warpline_lum_affine_bilinear_clipped(src, dst[row], xc, yc, ai0, ai3, width, height, blank);
      /*Move coordinates one step further along*/
      xc += ai1; yc += ai4;
    }
  }

  /*Warp into the (height x width) image dst, from the source image src.
  The pixel mapping is
  dst[y][x] = src(A[x y 1]) = src[Ax[1]][Ax[0]]
  A is an affine mapping from dst to src.
  The function assumes src and dst have the same dimensions, i.e., width and height.
  If patch touches outside of source image, the warp will be clipped
  */
  template<typename T>
  void i_warp_lum_affine_bilinear(const unsigned char *const *src, const T A[6], unsigned char **dst, int width, int height, unsigned char blank = I_DEFAULT_WARP_BLANK_VALUE_UCHAR)
  {
    int Aint[6];
    /*We need image coordinates in the source from which to take pixel (x,y),
    truncated, as well as the fractions in order to do bilinear interpolation.
    In order to avoid float to int conversion at every pixel, and in order to
    do this in a predictable way, the mapped coordinates are multiplied by 2^k
    and converted to int's. This way, the first k bits (starting from left) are fractional bits
    that can be used to infer the bilinear blending, and the remaining bits
    are actual pixel coordinates. In other words, we keep track of
    2^k*A[x y 1] in integers, and then mask out for blending and shift down
    for coordinates. In this way, the first column of 2^k*A gives increment with
    the x coordinate, the second column with the y coordinate, and the last column
    the starting coordinate*/
    i_warp_affine_convert_to_int<T>(A, Aint);

    if (i_warp_affine_is_inside(Aint, width, height))
    {
      i_warp_lum_affine_bilinear_unclipped(src, Aint, dst, width, height);
    }
    else
    {
      i_warp_lum_affine_bilinear_clipped(src, Aint, dst, width, height, blank);
    }
  }

  /*Rotate an image by 90 degree in the clockwise direction*/
  template<typename T>
  inline void i_rotate_lum_cw_90(const T* src, T *dst, int src_width, int src_height)
  {
    int x, y;
    const T *cptr_src = src;
    T* ptr_dst = NULL;
    int heightm1 = src_height - 1;
    for (y = 0; y < src_height; ++y)
    {
      ptr_dst = dst + heightm1 - y;
      for (x = 0; x < src_width; ++x)
      {
        *ptr_dst = cptr_src[x];
        ptr_dst += src_height;
      }
      cptr_src += src_width;
    }
  }

  /*Rotate an image by 90 degree in the counter-clockwise direction*/
  template<typename T>
  inline void i_rotate_lum_ccw_90(const T* src, T *dst, int src_width, int src_height)
  {
    int x, y;
    const T *cptr_src = src;
    T* ptr_dst = NULL;
    int widthm1xheight = (src_width - 1)*src_height;
    for (y = 0; y < src_height; ++y)
    {
      ptr_dst = dst + widthm1xheight + y;
      for (x = 0; x < src_width; ++x)
      {
        *ptr_dst = cptr_src[x];
        ptr_dst -= src_height;
      }
      cptr_src += src_width;
    }
  }

}/* namespace idl */